#include"IMovingSphere.h"

IMovingSphere::IMovingSphere( CollisionManager* cm )
{
	if ( cm == NULL )
		throw "NULL POINTER EXCEPTION ON -IMovingSphere::IMovingSphere( CollisionManager* cm )-";
	_cm = cm;
	_detectCollision = true;
}

void IMovingSphere::setCollisionDetection( bool detect )
{
	_detectCollision = detect;
}

bool IMovingSphere::isCollisionDetection(  ) const
{
	return _detectCollision;
}

Vector3 IMovingSphere::GetMovement(  )
{
	if( _cm == NULL )
		throw "CollisionManger instance is NULL on IMovingSphere::MOVE(  )";

	#ifdef DEBUG2
	cout <<"==] COLLISION_DETECTION~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ "<<endl;
	#endif
	
	// set the estimateMove of the original movement
	estimateMove = getDir();

	if( !_detectCollision )
		return estimateMove;
	
	// print size of sub huls
	map<int,CollisionHul*>* huls = _cm->getChildHuls();

	// begin with recursive collision checking
	CheckHuls( huls );

	// send the super class the estimateMove
	return estimateMove;
}

////////////////////////////////////////////////////////////////////////////////////////
// recursive method runs through all huls ( inclusive inwards ) and calls the 
// collision checking methods for each.
void IMovingSphere::CheckHuls( map<int,CollisionHul*>* huls )
{
#ifdef DEBUG
	cout <<"==] CHILD_HULS_SIZE~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ "<<huls->size()<<endl<<endl;
#endif
	map<int,CollisionHul*>::iterator it = huls->begin();
	while	( it != huls->end() )
	{
#ifdef DEBUG		
		cout <<"==] \tCOLLISION_STEP_[1] {IS_IN_HUL} "<<endl;
#endif
		if( isInHul( it->second ) )
		{
#ifdef DEBUG
			cout <<"==] \t\tIN HULL ==> CHECKING HUL"<<endl;
#endif
			CheckHuls( it->second->getChildHuls() );
			CheckPlanesCollision( it->first, it->second );
		}
		it++;
	}
}

inline bool IMovingSphere::isInHul( CollisionHul* hul )
{
	// check if this sphere  goes to intersect with the hul
	return (( hul->getCenter() - getPos() ).getAbsolute()) < (getRadius()+hul->getRadius());
}

void IMovingSphere::CheckPlanesCollision( int id, CollisionHul* hul )
{
	list<TrianglePlane*>* planes = hul->getTrianglePlaneList();
#ifdef DEBUG
	cout <<"==] ["<<id<<"] PLANES_SIZE: ________________________"<<planes->size()<<endl;
#endif
	
	list<TrianglePlane*>::iterator it = planes->begin();
	while	( it != planes->end() )
	{
#ifdef DEBUG		
		cout <<"==] TRIANGLE [=============================="<<endl;		
		cout <<"==] \t\tP1| "<<(*it)->getP1()<<endl;		
		cout <<"==] \t\tP2| "<<(*it)->getP2()<<endl;		
		cout <<"==] \t\tP3| "<<(*it)->getP3()<<endl;		
		cout <<"==] \t\tNO| "<<(*it)->getNormal()<<endl;	
		cout <<"==] \tCOLLISION_STEP_[2] {ALIGNMENT_BY_SCALAR} "<<endl;
#endif
		float scal = scalar( *it );
		if( scal < -GeoMath::EPS )// wenn wir uns auf die vorderseite der flaeche zu bewegen
		{
			// abstand zu ebene mit neuer position
#ifdef DEBUG			
			cout <<"==] \tCOLLISION_STEP_[3] {NEW_PLANE_DISTANCE} "<<endl;
#endif
			float d  = distanceToPlane( *it, getPos() -  ( (*it)->getNormal() * getRadius() ) +  estimateMove  );			
			if( d < 0.0  )// wenn neue position hinter flaeche
			{			
#ifdef DEBUG				
				cout <<"==] \tCOLLISION_STEP_[4] {CURRENT_PLANE_DISTANCE} "<<endl;
#endif
				float d1 = distanceToPlane( *it, getPos() +  ( (*it)->getNormal() * getRadius() ) );
				// wenn distanz kleiner als 2 mal radius => kugel schnitt ebene und ( funzt nich, sonst bleib ich hängen -_-)
				// wenn jetziger abstand vor der ebene				
				if( /*(GeoMath::Abs(d) < (2*getRadius()) ) ||*/ ( d1 > 0.0 ) )
				{
					// abstand zu ebene mit alter position
					Vector3 rayPos(getPos() -  ( (*it)->getNormal() * getRadius() ));					
					Vector3 intersect;
#ifdef DEBUG					
					cout <<"==] \tCOLLISION_STEP_[5] {PLANE_TRI_INTERSECTION} "<<endl; 	
#endif	
					if( GetIntersectionWithTriangle( *it,rayPos, scal, intersect ) )
					{// schnitt mit dreieck => kollision behandeln!
#ifdef DEBUG2						
						cout <<"==] \t- - - - - COLLISION_STEP_[6] {COLLISION_DETECTED=>HANDLE_IT} "<<endl; 
						cout <<"==] \tOLD_ESTIMATE: ["<<estimateMove<<"] "<<endl;			
						cout <<"==] \tINTERSECT: ["<<intersect<<"] "<<endl;			
						cout <<"==] \tNORMAL: ["<<(*it)->getNormal()<<"] "<<endl;		
						cout <<"==] \tD: ["<<d<<"] "<<endl;	
						cout <<"==] \tD1: ["<<d1<<"] "<<endl;	
#endif
						HandleCollision( (*it)->getNormal(), intersect, rayPos );
					}
					else
					{
#ifdef DEBUG							
						cout <<"==] \tCOLLISION_STEP_[7] {CHECK_EDGE_INTERSECTION} "<<endl; 	
#endif
						// kollisionsberechnung mit den kanten
						CheckAndHandleEdge((*it)->getP1(), (*it)->getEdgeRayP1(),(*it)->getEdge1() );
						CheckAndHandleEdge((*it)->getP2(), (*it)->getEdgeRayP2(),(*it)->getEdge2());
						CheckAndHandleEdge((*it)->getP3(), (*it)->getEdgeRayP3(),(*it)->getEdge3());
					}
				}
			}
		}
		//else if( scal < GeoMath::EPS )
		//{
			// wir bewegen uns parallel mit der flaeche gegen eine ihrer kanten 
			// dieser fall wird zunaechst nicht beachtet.
			//
			//
			//			-->		
			//      * 			|------|
			//	 *     *		|      |
			//   *     *		|      |
			//      *			|------|
			//
			//
		//}
		it++;
	}
}

inline float IMovingSphere::scalar( TrianglePlane* plane )
{
#ifdef DEBUG	
	cout <<"==] \tSKALAR: "<<plane->getNormal()*(estimateMove.getNormal())<<endl;
#endif
	return plane->getNormal()*estimateMove;
}


inline float IMovingSphere::distanceToPlane( const TrianglePlane* plane, const Vector3& p)
{
	
	float d = 
	(
		plane->getNormal() * 
			( 
				p - plane->getP1() 
			)
	);
#ifdef DEBUG	
	cout <<"==] \tP1: ["<<plane->getP1() <<"] "<<endl;
#endif
	return d;
}

inline bool IMovingSphere::GetIntersectionWithTriangle(const TrianglePlane* plane, const Vector3& rayPos,  const float& scal, Vector3& intersect  )
{
	if( scal == 0.0 )
		return false;
	
	
	if( !planeLineIntersect (rayPos, plane->getDistance(), estimateMove,plane->getNormal(),scal,intersect ) )
		return false;
	Vector3 vec1 = (plane->getP1() - intersect);
	Vector3 vec2 = (plane->getP2() - intersect);
	Vector3 vec3 = (plane->getP3() - intersect);
	float summAngle = vec1.getAngle(vec2);
	summAngle += vec2.getAngle( vec3 );
	summAngle += vec3.getAngle( vec1 );
#ifdef DEBUG
	cout <<"==] \tINTERSECT: ["<<intersect<<"] "<<endl;
	cout <<"==] \tANGLE: ["<<GeoMath::RadToDeg(summAngle)<<"] "<<endl;
#endif
	return GeoMath::Abs ((2*GeoMath::PI) - summAngle) < GeoMath::EPS;
}

inline bool IMovingSphere::planeLineIntersect( 
							const Vector3& rayOrigin, const float& planeDistance, 
							const Vector3& rayDir,const Vector3& planeNormal, const float& scal, Vector3& intersect)
{
	// rayPos : 	Position des StrahlStart Punktes an der Huelle der Kugel
	// disPlane:	Distanz von der Plane zum NullPunkt
	// disSphere:	Distanz vom EndPunkt vom Schussstrahl.
	// scal:		Scalar Wert Vom SchussStrahl und Normale der Plane
	// intersect:	Schnittpunkt an der Plane
	float lamda = ( ( ( planeDistance - ( rayOrigin * planeNormal )) / scal ) - GeoMath::EPS );
#ifdef DEBUG2
	cout <<"==] \tLAMDA: ["<<lamda<<"] "<<endl;
#endif
	if( lamda < -1.0 || lamda > 1.0 )
		return false;
	intersect = rayOrigin + ( rayDir * lamda) ;
	return true;
}

void IMovingSphere::HandleCollision( const Vector3& normal, const Vector3& intersect, const Vector3& rayPos )
{
#ifdef DEBUG
	cout <<"==] HANDLE_DISTANCE: "<<endl;
	cout <<"==] \tNORMAL: "<<normal<<endl;
	cout <<"==] \tINTERSECT: "<<intersect<<endl;
#endif
	// gleitebene berechnen!
	Vector3 newMove = intersect - rayPos;
	Vector3 lostMove = newMove - estimateMove;
	Vector3 planeVec = lostMove.projectionOn( normal );
	//planeVec = planeVec + lostMove;
	//~ Vector3 planeVec = lostMove.vecProduct(normal);
	//~ planeVec = Matrix3::Rotate( normal, GeoMath::DegToRad( 90.0 ) ) * planeVec;	
	//~ planeVec.normalize();
	//~ planeVec.setLength( lostMove * planeVec );
	estimateMove += planeVec;
#ifdef DEBUG2
	if( estimateMove.getY() > 0.01 )
		cout <<"==] \t\t\t\t\tESTIMATE: "<<estimateMove<<endl;
	else
		cout <<"==] \tESTIMATE: "<<estimateMove<<endl;
#endif
	//cout <<"==] \ESTIMATE: "<<estimateMove<<endl;
};

void IMovingSphere::CheckAndHandleEdge(const Vector3& origin, const Vector3& dir, const EdgeInfo& edgeData)
{
	// Formel fuer berechnung der Schnittpunkte von einem Strahl und einem Kugel
	// Kugel :	(x - xM)2 + (y - yM)2 + (z - zM)2 = r2
	// Strahl:	R(s) = Origin + (s * Dir)
	// Formeln durch substitution zusammengesetzt => qudratische gleichung ...
	// ... mittels pq formel die lösung ausrechnen. 
	Vector3 newPosition = getPos () + estimateMove;
#ifdef DEBUG	
	cout <<"==] \t\tEDGE(O#V): "<<origin<<"#"<<dir<<endl; 
#endif
	
	float a = dir.getSquare();
	float b = 2 * (dir * (origin - (newPosition)));
	float c = (newPosition - origin).getSquare() - GeoMath::square( getRadius () );
	
	float inSqrt = GeoMath::square(b) - (4*a*c);
	if( inSqrt < 0.0 )
	{
#ifdef DEBUG
		
		cout <<"==] \t\t\tS1: --"<<endl;
		cout <<"==] \t\t\tS2: --"<<endl;	
#endif
		return;
	}	
	float squareRoot = GeoMath::Sqrt( inSqrt ); 
	float s1 = (-b + squareRoot) / (2.0*a);
	float s2 = (-b - squareRoot) / (2.0*a);
	
	float minS = GeoMath::Min(s1,s2);
	Vector3 S = origin + ( dir * minS );	
	
	if( minS >= 0.0 && minS <= 1.0 )
	{// Schnitt mit der Kante!!! => Kollision behandeln		
		Vector3 projection = newPosition - S;
		projection =  projection.projectionOn( dir );
		
		Vector3 normal;
		if( edgeData._isSmooth )
		{
			normal = edgeData._normal;
#ifdef DEBUG
			cout <<"_____smooth______"<<endl;
#endif
		}
		else
		{
			normal =  (newPosition - ( S + projection ));
			normal.normalize();		
#ifdef DEBUG
			cout <<"_____not_smooth______"<<endl;
#endif
		}
		
		Vector3 rayPos = getPos () - (normal * getRadius());
		Vector3 intersect;
		if( !planeLineIntersect( rayPos, normal*(S + projection), estimateMove,
			normal, normal*estimateMove.getNormal(),intersect
		))
			return;
#ifdef DEBUG2
	cout <<"===================================] \t\t\tMinS: "<<minS<<endl;
#endif		
#ifdef DEBUG2	
		cout <<"==] - - - - - - - - - - - - - - - - - [=="<<endl;
		cout <<"==] \t\t\tINTERSECT: ["<<intersect<<"] "<<endl;
		cout <<"==] \t\t\tNORMAL: ["<<normal<<"] "<<endl;
		cout <<"==] \t\t\tSPHERE_INTER: ["<<rayPos<<"] "<<endl;
		cout <<"==] \t\t\tS1: "<<s1<<endl;
		cout <<"==] \t\t\tS2: "<<s2<<endl;		
#endif
		
		HandleCollision( normal, intersect, rayPos );
	}
#ifdef DEBUG
	cout <<"==] - - - - - - - - - - - - - - - - - [=="<<endl;
	cout <<"==] \t\t\tS1: "<<s1<<endl;
	cout <<"==] \t\t\tS2: "<<s2<<endl;
#endif			

}


